/* rotinas_obstaculos.c -- Rotinas básicas para serem executadas diretamente nos clientes
 *
 * Copyright (C) 2008, Diogo F. S. Ramos <diogofsr@grad.icmc.usp.br>
 *                     Bruno Ribeiro de Oliveira
 *                     Arineiza Cristina Pinheiro
 *                     Tomas Grover
 * 
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 */

/** @file
 * Rotinas básicas para serem executadas diretamente nos clientes
 */

#include <stdio.h>
#include <libplayerc/playerc.h>
int main(int argc, const char **argv)
{
  int i, k;
  float turn_speed=0;

  //Cliente para conexão com o servidor
  playerc_client_t *client;

  //Objeto para conexão com o odômetro
  playerc_position2d_t *position2d;

  //Objeto para conexão com o laser
  playerc_laser_t *laser;

  //Objeto para os obstaculos
  playerc_blobfinder_t *bf;

  //Objeto para a garra
  playerc_gripper_t *gripper;

  float vel=0, rotate=0;

 //Conecta ao servidor no endereço “localhost”, na porta 6665 (default)
  client = playerc_client_create(NULL, "localhost", 6665);
  if (playerc_client_connect(client) != 0)
    return -1;

  //Conecta ao odômetro
  position2d = playerc_position2d_create(client, 0);
  if (playerc_position2d_subscribe(position2d, PLAYERC_OPEN_MODE) != 0)
  {
    fprintf(stderr, "error: %s\n", playerc_error_str());
    return -1;
  }

  //Conecta ao laser	
  laser = playerc_laser_create(client, 0);
  if (playerc_laser_subscribe(laser, PLAYERC_OPEN_MODE))
    return -1;

  //Conecta ao blobfinder
  bf = playerc_blobfinder_create(client, 0);
  if (playerc_blobfinder_subscribe(bf, PLAYER_OPEN_MODE))
    return -1;

  //Conecta a garra
  gripper = playerc_gripper_create(client, 0);
  if (playerc_gripper_subscribe(gripper, PLAYER_OPEN_MODE))
    return -1;

  playerc_position2d_enable(position2d, 1);


  //Aqui eh p/ ter as duas rotinas
  //1) desviar de obstaculos
  //2) reconhecer a caixa e pega-la
  while(1)
  {

    playerc_client_read(client);

    //Rotina para desviar de obstaculos
    if (laser->scan[180][0] < 1.00)
	vel=0;
    else if (laser->scan[180][0] < 1.5)
	vel=0.2;
    else
	vel=0.2;
    if ((laser->scan[90][0] < 1.8) || (laser->scan[0][0] < 1.3) || (laser->scan[45][0] < 1.8 ) || laser->scan[135][0] < 1.8))
	rotate=0.6;
    else if ((laser->scan[270][0] < 1.8) || (laser->scan[360][0] < 1.3) || laser->scan[315][0] < 1.8 ) ||  laser->scan[225][0]))
	rotate=-0.6;
    else if (vel==0)
	rotate=0.6;
    else
	rotate=0;

    //Rotina para reconhecer os obstaculos
    if (bf->blobs_count > 0)
	for(k=0; k < bf->blobs_count; k++)
	if (bf->blobs[k].color==65280)
	{
		if(bf->blobs[k].x<38)
			turn_speed=0.2;
		else if(bf->blobs[k].x>42)
			turn_speed=-0.2;
		else
			turn_speed=0.0;
		printf("\nBlob x:%u  ", bf->blobs[k].x);
	}
    playerc_position2d_set_cmd_vel(position2d, vel, 0, rotate, 1);

  }

  //Garra "agarrando"
  playerc_position2d_set_cmd_vel(position2d, 0, 0, 0, 1);

	playerc_gripper_set_cmd(gripper, GRIPopen, 0);
	playerc_gripper_set_cmd(gripper, GRIPclose, 0);

